#include <iostream>
#include <stdio.h>
#include "cv.h"
#include "highgui.h"

using namespace std;

IplImage* doPyrDown(IplImage* in, int filter = IPL_GAUSSIAN_5x5){
	//assert(in->width%2 == 0 && in->height%2 == 0); //make sure image is divisable by two
	IplImage* out = cvCreateImage(
			cvSize(in->width/2, in->height/2 ), //note cvSize is not a class CvSize is a class -go
			in->depth,
			in->nChannels
			);
	cvPyrDown(in, out);
	return(out);
};

	IplImage* doCanny(IplImage* in, double lowThresh, double highThresh, double aperture){
		if(in->nChannels != 1)
			return(0); //Canny only handles gray scale images
		IplImage* out = cvCreateImage(
				CvSize(cvGetSize(in)), //note cvSize is not a class CvSize is a class -go
				IPL_DEPTH_8U,
				1
				);
		cvCanny(in, out, lowThresh, highThresh, aperture);
		return(out);
	};

IplImage* doGrayScale(IplImage* in){
	IplImage* out = cvCreateImage( //note cvSize is not a class CvSize is a class -go
			CvSize(cvGetSize(in)),
			IPL_DEPTH_8U,
			1
			);
	cvCvtColor(in, out, CV_RGB2GRAY);
	return(out);
};

void mapImage(IplImage* src, IplImage* dst)
{
	CvScalar s;

	int maxGreen = 0;
	int minBlue = 100000;
	int nl= src->height; // number of lines
	int nc= src->width;
	float min =100000000;
	for (int i=0; i<nl; i++) {
		for (int j=0; j<nc; j++) {
			// process each pixel ---------------------
			s = cvGet2D(src, i, j);
			//printf("B=%f, G=%f, R=%f\n",s.val[0],s.val[1],s.val[2]);
			s.val[0] = (255-s.val[1])*(255-s.val[1]) + s.val[2]*s.val[2] + s.val[2]*s.val[2];
			s.val[0] /= 200;
			cvSet2D(dst, i, j, s);
			if (min > s.val[0]){
				printf("B=%f, G=%f, R=%f\n",s.val[0],s.val[1],s.val[2]);
				min = s.val[0];
			}
			// end of pixel processing ---------------
		} // end of line
	}
	printf("G=%f\n",min);
	/*for(int i = 0; i < cvGetSize(src).width*cvGetSize(src).height; i++)
	  {
	//img_8uc1->imageDataOrigin[i] = img_8uc1->imageDataOrigin[i] + 45;
	//printf("green %d: %d\n", i, img_8uc1->imageDataOrigin[i]);
	int totaldiff = 0;
	int diff = 127- g->imageDataOrigin[i];
	if (g->imageDataOrigin[i] > maxGreen)
	maxGreen = g->imageDataOrigin[i];
	if (b->imageDataOrigin[i] < minBlue)
	minBlue = b->imageDataOrigin[i];
	totaldiff+= diff*diff;
	diff = r->imageDataOrigin[i] + 128;
	totaldiff+= diff*diff;
	diff = b->imageDataOrigin[i] + 128;
	totaldiff += diff*diff;
	int greenness = (totaldiff/20) - 128;
	if (r->imageDataOrigin[i] > b->imageDataOrigin[i]  && b->imageDataOrigin[i] + 128 > b->imageDataOrigin[i] + 128)
	greenness = 127;
	else
	greenness = -128;
	//int greenness = 255*(g->imageDataOrigin[i]*g->imageDataOrigin[i])/(1+r->imageDataOrigin[i]*r->imageDataOrigin[i] + b->imageDataOrigin[i]*b->imageDataOrigin[i]);
	if (greenness > 127)
	{
	cout << "Overkill!\n";
	greenness = 127;
	}
	if (greenness < -128){
	cout << "Underkill!\n";
	greenness = -128;
	}		
	dst->imageDataOrigin[i] = greenness;
	//dst->imageDataOrigin[i] = r->imageDataOrigin[i];
	}*/
};

void contourTest(char* image)
{
	cvNamedWindow( "Contours", 1 );
	IplImage* img_orig = cvLoadImage( image);

	IplImage* img_8uc1 = cvCreateImage( cvGetSize(img_orig), IPL_DEPTH_8U, 1);

	IplImage* img_edge = cvCreateImage( cvGetSize(img_orig), 8, 1 );
	IplImage* img_8uc3 = cvCreateImage( cvGetSize(img_orig), 8, 3 );

	mapImage(img_orig, img_8uc1);

	cvThreshold( img_8uc1, img_edge, 200, 255, CV_THRESH_BINARY );

	CvMemStorage* storage = cvCreateMemStorage();
	CvSeq* first_contour = NULL;

	int Nc = cvFindContours(
			img_edge,
			storage,
			&first_contour,
			sizeof(CvContour),
			CV_RETR_LIST );

	int n=0;
	printf( "Total Contours Detected: %d\n", Nc );
	CvScalar red = CV_RGB(250,0,0);
	CvScalar blue = CV_RGB(0,0,250);

	for( CvSeq* c=first_contour; c!=NULL; c=c->h_next ){
		CvPoint top, left, right, bottom;
		cvCvtColor( img_8uc1, img_8uc3, CV_GRAY2BGR );
		cvDrawContours(
				img_8uc3,
				c,
				red,		// Red
				blue,		// Blue
				1,		// Vary max_level and compare results
				2,
				8 );
		printf( "Contour #%d\n", n );
		cvShowImage( "Contours", img_8uc3 );
		printf( " %d elements:\n", c->total );
		top = left = right = bottom = *CV_GET_SEQ_ELEM(CvPoint, c, 0);
		for( int i=0; i < 10000; ++i ){
			CvPoint* p = CV_GET_SEQ_ELEM( CvPoint, c, i );
			if (p == NULL) {cout << "done with contour\n";break;}
			if (p->y < bottom.y)
				bottom = *p;
			if (p->y > top.y)
				top = *p;
			if (p->x < left.x)
				left = *p;
			if (p->x > right.x)
				right = *p;
		}
		if (top.y - bottom.y > 10 && right.x - left.x > 10)
		{
			printf("Boundsing points: \nx: %d y: %d\nx: %d y: %d\nx: %d y: %d\nx: %d y: %d\n",
				top.x, top.y, left.x, left.y, bottom.x, bottom.y, right.x, right.y);
		}
		cvWaitKey();
		n++;
	}

	printf( "Finished all contours.\n");
	cvCvtColor( img_8uc1, img_8uc3, CV_GRAY2BGR );
	cvShowImage( "Contours", img_8uc3 );
	cvWaitKey();

	cvDestroyWindow( "Contours" );

	cvReleaseImage( &img_8uc1 );
	cvReleaseImage( &img_8uc3 );
	cvReleaseImage( &img_edge );
};


int displayWebcam()
{
	CvCapture * capture = 0;
	IplImage * frame = 0;
	IplImage* out = 0;
	int key = 0;
	capture = cvCaptureFromCAM(0);
	cvNamedWindow("Webcam", CV_WINDOW_AUTOSIZE);

	if (!capture){
		return 1;
	}
	while (key != 'q')
	{
		frame = cvQueryFrame( capture);
		/*out = cvCreateImage(
		  cvSize(frame->width, frame->height ), //note cvSize is not a class CvSize is a class -go
		  frame->depth,
		  frame->nChannels
		  );*/
		if (!frame) break;
		//cvShowImage( "Webcam", out);
		key = cvWaitKey(1);

	}
}

void displayImageTest(char* image)
{
	IplImage* in_img = cvLoadImage(image);
	cvNamedWindow("ExampleMany-in");
	cvNamedWindow("Smoothed");
	cvNamedWindow("Shrunk");
	cvNamedWindow("GrayScale");
	cvNamedWindow("Canny");

	cvShowImage("ExampleMany-in", in_img);
	IplImage* out_img = cvCreateImage(
			cvGetSize(in_img),
			IPL_DEPTH_8U,
			3
			);
	cvSmooth(in_img, out_img, CV_GAUSSIAN, 3, 3);
	cvShowImage("Smoothed", out_img);
	cvShowImage("Shrunk", doPyrDown(in_img));
	cvShowImage("GrayScale", out_img = doGrayScale(in_img));
	cvShowImage("Canny", doCanny(out_img, 10, 100, 3)); //values the book had for their edge detection -go
	cvReleaseImage(&in_img);
	cvReleaseImage(&out_img);
	cvWaitKey(0);
	cvDestroyWindow("ExampleMany-in");
	cvDestroyWindow("Smoothed");
	cvDestroyWindow("Shrunk");
	cvDestroyWindow("GrayScale");
	cvDestroyWindow("Canny");

};

int 
main(int argc, char** argv){
	//displayImageTest(argv[1]);
	//displayWebcam();
	contourTest(argv[1]);
	return 0;
};

